#include "Math.h"

float rnd(float b,float e)
{
	return (b+e)*0.5f+((rand()%20000)-10000)*(e-b)/10000.0f;
}

float rnd(float e)
{
	return e*0.5f+((rand()%20000)-10000)*e/10000.0f;
}

float rnd_s(float b,float e)
{
	if(abs(b-e)<0.00001f) return b;
	else
		return (b+e)*0.5f+((rand()%20000)-10000)*(e-b)/10000.0f;
}

int rand(int a)
{
	return (rand()%(a+1));
}

int rand(int b,int e)
{
	return (rand()%((e-b)+1))+b;
}

int rand_s(int b,int e)
{
	if(b==e) return b;
	else
		return (rand()%((e-b)+1))+b;
}

float Dist2D(float x,float y)
{
	return sqrt(x*x+y*y);
}

float Dist2D(float x1,float y1,float x2,float y2)
{
	return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
}

unsigned int ToPow2(unsigned int val)
{
	if(val>4096)return 8192;
	if(val>2048)return 4096;
	if(val>1024)return 2048;
	if(val>512) return 1024;
	if(val>256) return 512;
	if(val>128) return 256;
	if(val>64)  return 128;
	if(val>32)  return 64;
	if(val>16)  return 32;
	if(val>8)	return 16;
	if(val>4)	return 8;
	if(val>2)	return 4;
	return val;
}

inline void SinCos(float a,float & s,float & c )
{
#ifdef _WIN32
	_asm {
		fld		a
		fsincos
		mov		ecx, c
		mov		edx, s
		fstp	dword ptr [ecx]
		fstp	dword ptr [edx]
	}
#else
	s = sinf( a );
	c = cosf( a );
#endif
}

inline float InvSqrt (float x)
{
    float xhalf = 0.5f*x;
    int i = *(int*)&x;
    i = 0x5f3759df - (i>>1);
    x = *(float*)&i;
    return x*(1.5f - xhalf*x*x);
}


float AngleDist(float a1,float a2)
{
	if(a1-a2>180.0f)
		return  360.0f - (a1-a2);
	
	if(a1-a2<-180.0f)
		return 360.0f + (a1-a2);

	return a1-a2;
}

float AngleLerp(float a1,float a2,float scalar)
{
	return a1+AngleDist(a2,a1)*scalar;
}

float AngleNormalize(float a)
{
	int ia = (int)a;
	float scalar = a-ia;
	ia %= 360;
	a = ia+scalar;

	if(a<-180)
		return a+360;
	if(a>180)
		return a-360;

	return a;
}

int round(float a)
{
	int c = (int)a;
	float scalar = a-c;

	if(scalar<0.5f)
		return c;
	else
	{
		if(c<0)
			return c-1;
		else
			return c+1;
	}
}

#include "math/Vector2D.cpp"
#include "math/Vector3D.cpp"
#include "math/Vector4D.cpp"
#include "math/Matrix3D.cpp"
#include "math/Matrix4x4.cpp"
#include "math/Quaternion.cpp"









inline void V_Add(Vector3D &v1, const Vector3D &v2)
{
	v1.x += v2.x;
	v1.y += v2.y;
	v1.z += v2.z;
}

inline void M_Mult(Vector3D &result, const Matrix4x4 &m, const Vector3D &v)
{
	result.x = m.m[0][0] * v.x + m.m[1][0] * v.y + m.m[2][0] * v.z + m.m[3][0];
	result.y = m.m[0][1] * v.x + m.m[1][1] * v.y + m.m[2][1] * v.z + m[3][1];
	result.z = m.m[0][2] * v.x + m.m[1][2] * v.y + m.m[2][2] * v.z + m.m[3][2];
}

inline void M_MultAdd(Vector3D &result, const Matrix4x4 &m, const Vector3D &v)
{
	result.x += (m.m[0][0] * v.x + m.m[1][0] * v.y + m.m[2][0] * v.z + m.m[3][0]);
	result.y += (m.m[0][1] * v.x + m.m[1][1] * v.y + m.m[2][1] * v.z + m[3][1]);
	result.z += (m.m[0][2] * v.x + m.m[1][2] * v.y + m.m[2][2] * v.z + m.m[3][2]);

}
inline void M_Mult(Matrix4x4 &result, const Matrix4x4 &m, float scalar)
{
	for(int i=0;i<4;++i)
		for(int j=0;j<4;++j)
			result.m[i][j] = m.m[i][j]*scalar;
}

inline void vec3lerp (Vector3D &result, const Vector3D& a, const Vector3D& b, float t )
{
	result.x = a.x + (b.x - a.x) * t;
	result.y = a.y + (b.y - a.y) * t;
	result.z = a.z + (b.z - a.z) * t;
}


static	void	getCubeVector ( int side, int cubeSize, int x, int y, Vector3D& v )
{
    float	s  = ((float) x + 0.5f) / (float) cubeSize;
    float	t  = ((float) y + 0.5f) / (float) cubeSize;
    float	sc = 2*s - 1;
    float	tc = 2*t - 1;

    switch ( side )
    {
        case 0:
            v = Vector3D ( 1, -tc, -sc );
            break;

        case 1:
            v = Vector3D ( -1, -tc, sc );
            break;

        case 2:
            v = Vector3D ( sc, 1, tc );
            break;

        case 3:
            v = Vector3D ( sc, -1, -tc );
            break;

        case 4:
            v = Vector3D ( sc, -tc, 1 );
            break;

        case 5:
            v = Vector3D ( -sc, -tc, -1 );
            break;
    }

    v.normalize ();
}


#include "sigel_math.h"